function op = draw(q,length)

NBODIES = max(size((q)));

hold off

plot(0,0,'rO','LineWidth',3);
p1 = [0 0]';
for i=1:NBODIES                     
    
    % Transformation matrix between body and newtonian frame
    n_c_k(:,:,i) = [cos(sum(q(1:i))) -sin(sum(q(1:i)));
                     sin(sum(q(1:i))) cos(sum(q(1:i)))];                     
    
    p2 = n_c_k(:,:,i)*[length(i) 0]';
    p3 = p1+p2;
    plot(p1(1),p1(2),'rO','LineWidth',3);
    line([p1(1) p3(1)], [p1(2) p3(2)],'LineWidth',2);
    hold on
    p1 = p3;
    
end

axis([-10 10 -10 10]);